#include "../Global/GlobalDefs.hpp"
#include "../Global/globalVars.hpp"

#include "../TempControl/temperatureControllers.hpp"

uint8_t BangBangController::calculate(float targetTemp, float currentTemp, clock_t time, bool verbose)
{
	float tempP = targetTemp - currentTemp;
	if (direction < 0) {
		tempP = -tempP;
	}
	if (tempP < ptTempDiffC[0])
	{
		return ptDrivePercent[0];
	}
	else if (tempP < ptTempDiffC[1])
	{
		return ptDrivePercent[1];
	}
	else if (tempP < ptTempDiffC[2])
	{
		return ptDrivePercent[2];
	}
	else if (tempP < ptTempDiffC[3])
	{
		return ptDrivePercent[3];
	}
	else
	{
		return ptDrivePercent[4];
	}
}

bool BangBangController::update(long* curve, int curveLen)
{
	if (curveLen != curvePtNum * 2)
	{
		Serial.print("Must have ");
		Serial.print(curvePtNum);
		Serial.println(" points, each with temp then rate");
		return false;
	}
	for (int ptIdx = 0; ptIdx < curvePtNum; ptIdx++)
	{
		ptTempDiffC[ptIdx] = curve[ptIdx * 2] / 100.0f;
		ptDrivePercent[ptIdx] = (uint8_t)curve[ptIdx * 2 + 1];
	}
	return true;
}

void PIDController::restart(clock_t time) {
	lastError = 0;
	lastTime = time;
	cumulatedError = 0;
}

clock_t PIDController::getTime()
{
	return lastTime;
}

void PIDController::putTime(clock_t time)
{
	lastTime = time;
}

uint8_t PIDController::calculate(float targetTemp, float currentTemp, clock_t time, bool verbose) {
	// i.e., the error
	//Serial.println("PIDController::calculate");
	float newError = targetTemp - currentTemp;
	float TempError = currentTemp - lastTemp;
	if (direction < 0) {
		newError = -newError;
		TempError = -TempError;
	}
	float timeDifference = max(1, int(time - lastTime)) / timeNormalier2Sec;
	GlobalVars::speed = TempError / timeDifference;
	//float lastCumuError = cumulatedError;
	float forgetTimeElapsed = timeDifference * errorForgetRatePerSec;
	//cumulatedError = cumulatedError * (1 - forgetTimeElapsed) + newError * forgetTimeElapsed;
	cumulatedError = cumulatedError  + newError * forgetTimeElapsed;
	float errorDifferential = newError - lastError;
	float outputSum = 0;
	float outputP = newError * kp;
	float outputI = cumulatedError * ki;
	//float outputI = min(cumulatedError * ki, (LIMIT_MAXSIZE_PID - outputP));
	float outputD = errorDifferential * kd / timeDifference;
	outputSum += outputP;
	outputSum += outputI;
	outputSum += outputD;
	GlobalVars::PIDoutputP = outputP;
	GlobalVars::PIDoutputI = outputI;
	GlobalVars::PIDoutputD = outputD;
	if (verbose) {
		Serial.print("PID: T@");
		Serial.print(time);
		Serial.print(",err=");
		Serial.print((int)(newError * 100));
		Serial.print(",cErr=");
		Serial.print((int)(cumulatedError * 100));
		Serial.print(",dErr=");
		Serial.print((int)(errorDifferential * 100));
		Serial.print(",oP=");
		Serial.print((int)(outputP * 100));
		Serial.print(",oI=");
		Serial.print((int)(outputI * 100));
		Serial.print(",oD=");
		Serial.print((int)(outputD * 100));
		Serial.print(",sum=");
		Serial.print((int)(outputSum * 100));
		Serial.println();
	}
	lastTime = time;
	lastTemp = currentTemp;
	lastError = newError;
	int ret = outputSum;
	return min(LIMIT_MAXSIZE_PID, max(LIMIT_MINSIZE_PID, ret));	
}